/*
*此文件用于解析和发送与上位机的协议帧，依赖于usb的cdc功能
*/
#include "ParaConfig.h"
#include "esp_log.h"
#include "string.h"
#include "nvs.h"
#include "ParameterManager.h"
#include "myusb.h"
static const char* TAG = "FFB_Protocol";
/**/
static ProtocolFrame_t rx_frame;
static uint8_t rx_buffer[128];
static uint32_t rx_index = 0;
static bool frame_started = false;
static DataStreamType_t active_streams = DATA_STREAM_NONE;

// 计算校验和
static uint8_t calculate_checksum(uint8_t* data, uint8_t length)
{
    uint8_t sum = 0;
    for (int i = 0; i < length; i++) {
        sum += data[i];
    }
    return sum;
}

// 发送协议帧
static void send_frame(uint8_t command, uint8_t* data, uint8_t data_length)
{
    size_t total_length = sizeof(ProtocolFrame_t) + data_length + sizeof(uint8_t);
    uint8_t* frame_buffer = (uint8_t*)malloc(total_length);
    ProtocolFrame_t* frame = (ProtocolFrame_t*)frame_buffer;
    
    frame->header = PROTOCOL_HEADER;
    frame->command = command;
    frame->data_length = data_length;
    
    if (data_length > 0 && data != NULL) {
        memcpy(frame->data, data, data_length);
    }
    
    // 计算校验和
    uint8_t checksum = calculate_checksum((uint8_t*)&frame->command, 
                                          1 + 1 + data_length);
    
    // 将校验和放在数据之后
    frame_buffer[total_length - 1] = checksum;
    
    // 发送
    safe_cdc_send(frame_buffer, total_length);
    
    free(frame_buffer);
}

// 处理接收到的数据
static void handle_protocol_command(ProtocolFrame_t* frame);
bool protocol_process_rx_data(uint8_t* data, uint32_t length)
{
    uint8_t checksum = 0;
    for (uint32_t i = 0; i < length; i++) {
        uint8_t byte = data[i];
        printf("%x\n",byte);
        if (!frame_started) {
            // 寻找帧头
            if (rx_index == 0 && byte == (PROTOCOL_HEADER & 0xFF)) {
                rx_buffer[rx_index++] = byte;
            } else if (rx_index == 1 && byte == (PROTOCOL_HEADER >> 8)) {
                rx_buffer[rx_index++] = byte;
                frame_started = true;
            } else {
                rx_index = 0; // 重置
            }
        } else {
            // 接收帧数据
            rx_buffer[rx_index++] = byte;
            
            // 检查是否收到足够数据来解析帧头信息（命令+数据长度）
            if (rx_index == 4) { // 已收到：header(2) + command(1) + data_length(1)
                // 现在可以知道完整帧的长度
                uint8_t data_len = rx_buffer[3]; // data_length在buffer[3]位置
                uint8_t total_frame_length = 4 + data_len + 1; // header(2)+command(1)+data_length(1)+data(n)+checksum(1)
                
                // 检查是否收到完整帧
                if (rx_index >= total_frame_length) {
                    // 解析帧
                    memcpy(&rx_frame.header, rx_buffer, 2);
                    rx_frame.command = rx_buffer[2];
                    rx_frame.data_length = rx_buffer[3];
                    
                    if (rx_frame.data_length > 0) {
                        memcpy(rx_frame.data, &rx_buffer[4], rx_frame.data_length);
                    }
                    
                    checksum = rx_buffer[4 + rx_frame.data_length];
                    
                    // 验证校验和
                    uint8_t expected_checksum = calculate_checksum(&rx_buffer[2], 
                                                                  rx_frame.data_length + 2);
                    
                    printf("Expected checksum: 0x%02X, Got: 0x%02X\n", 
                           expected_checksum, checksum);
                    
                    if (checksum == expected_checksum) {
                        // 处理有效帧
                        handle_protocol_command(&rx_frame);
                    } else {
                        ESP_LOGE(TAG, "Checksum error: expected 0x%02X, got 0x%02X", 
                                expected_checksum, checksum);
                    }
                    
                    // 重置接收状态
                    rx_index = 0;
                    frame_started = false;
                    memset(rx_buffer, 0, sizeof(rx_buffer));
                }
            } else if (rx_index > 4) {
                // 已经知道帧长度，检查是否完整
                uint8_t data_len = rx_buffer[3];
                uint8_t total_frame_length = 4 + data_len + 1;
                
                if (rx_index >= total_frame_length) {
                    // 解析帧（同上）
                    memcpy(&rx_frame.header, rx_buffer, 2);
                    rx_frame.command = rx_buffer[2];
                    rx_frame.data_length = rx_buffer[3];
                    
                    if (rx_frame.data_length > 0) {
                        memcpy(rx_frame.data, &rx_buffer[4], rx_frame.data_length);
                    }
                    
                    checksum = rx_buffer[4 + rx_frame.data_length];
                    
                    // 验证校验和
                    uint8_t expected_checksum = calculate_checksum(&rx_buffer[2], 
                                                                  rx_frame.data_length + 2);
                    
                    if (checksum == expected_checksum) {
                        handle_protocol_command(&rx_frame);
                    } else {
                        ESP_LOGE(TAG, "Checksum error: expected 0x%02X, got 0x%02X", 
                                expected_checksum, checksum);
                    }
                    
                    // 重置
                    rx_index = 0;
                    frame_started = false;
                    memset(rx_buffer, 0, sizeof(rx_buffer));
                }
            }
            
            // 防止缓冲区溢出
            if (rx_index >= sizeof(rx_buffer)) {
                ESP_LOGE(TAG, "RX buffer overflow");
                rx_index = 0;
                frame_started = false;
                memset(rx_buffer, 0, sizeof(rx_buffer));
            }
        }
    }
    
    return true;
}
static void handle_get_device_info(void)
{
    DeviceInfo_t info = {
        .fw_version = {1, 0, 0, 0},
        .device_id = 0x1234,
        .hardware_rev = 2,
        .serial_number = 0x56789ABC,
        .max_force = 10000, // 10N
        .axis_count = 2
    };
    
    send_frame(CMD_DEVICE_INFO_RESP, (uint8_t*)&info, sizeof(info));
}
// 发送操作结果
static void send_operation_result(uint8_t result)
{
    send_frame(CMD_OPERATION_RESULT, &result, 1);
}
// 处理协议命令
static void handle_protocol_command(ProtocolFrame_t* frame)
{
    switch (frame->command) {
        case CMD_RESET_DEVICE:
            //handle_reset_device();
            break;
            
        case CMD_GET_DEVICE_INFO:
            handle_get_device_info();
            break;
            
        case CMD_SAVE_PARAMS:
            save_all_parameters();
            break;
            
        case CMD_GET_ALL_PARAMS:
            //handle_get_all_params();
            break;
            
        case CMD_SET_SPRING_PARAMS:
            //handle_set_spring_params(frame->data, frame->data_length);
            break;
            
        case CMD_SET_DAMPER_PARAMS:
            //handle_set_damper_params(frame->data, frame->data_length);
            break;
        case CMD_SET_WHEEL_CENTER:
            printf("set center \n");    
        case CMD_PLAY_TEST_EFFECT:
            //handle_play_test_effect(frame->data, frame->data_length);
            break;
            
        case CMD_STOP_ALL_EFFECTS:
            //handle_stop_all_effects();
            break;
            
        case CMD_REALTIME_FORCE:
            //handle_realtime_force(frame->data, frame->data_length);
            break;
            
        case CMD_ENABLE_DATA_STREAM:
            //handle_enable_data_stream(frame->data, frame->data_length);
            break;
            
        case CMD_DISABLE_DATA_STREAM:
            //handle_disable_data_stream();
            break;
            
        default:
            ESP_LOGW(TAG, "Unknown command: 0x%02X", frame->command);
            send_operation_result(ERR_INVALID_CMD);
            break;
    }
}



static void handle_enable_data_stream(uint8_t* data, uint8_t length)
{
    if (length == 1) {
        active_streams = (DataStreamType_t)data[0];
        ESP_LOGI(TAG, "Data streams enabled: 0x%02X", active_streams);
        send_operation_result(ERR_SUCCESS);
    } else {
        send_operation_result(ERR_INVALID_DATA);
    }
}


// 定期调用此函数发送数据流
// void protocol_send_data_stream(void)
// {
//     static uint32_t last_send_time = 0;
//     uint32_t current_time = xTaskGetTickCount() * portTICK_PERIOD_MS;
    
//     if (current_time - last_send_time < 10) { // 100Hz最大频率
//         return;
//     }
    
//     last_send_time = current_time;
    
//     if (active_streams & DATA_STREAM_POSITION) {
//         PositionData_t pos_data = get_current_position_data();
//         send_frame(CMD_POSITION_DATA, (uint8_t*)&pos_data, sizeof(pos_data));
//     }
    
//     if (active_streams & DATA_STREAM_FORCE) {
//         ForceOutput_t force_data = get_current_force_output();
//         send_frame(CMD_FORCE_OUTPUT_DATA, (uint8_t*)&force_data, sizeof(force_data));
//     }
    
//     if (active_streams & DATA_STREAM_STATUS) {
//         // 发送设备状态数据
//         // send_frame(CMD_DEVICE_STATUS, ...);
//     }
// }